#include "symplectic_euler_cuda.h"
#include <cuda_block_size_set.h>

namespace PhysLeo
{
 
template<typename T>
__global__ static void K_SymplecticEulerCuda_advanceTime(glm::tvec3<T>* pos_ptr, glm::tvec3<T>* vel_ptr, glm::tvec3<T>* force_ptr, T* mass_ptr, T dt, int size)
{
    int pid = threadIdx.x + (blockIdx.x * blockDim.x);
    if (pid >= size) return;
    const auto vert_id = pid;

    #ifndef NDEBUG
    if(vert_id == 0)printf("before pos(%f,%f,%f) vel(%f,%f,%f) force(%f,%f,%f)\n",pos_ptr[vert_id][0],pos_ptr[vert_id][1],pos_ptr[vert_id][2],
    vel_ptr[vert_id][0],vel_ptr[vert_id][1],vel_ptr[vert_id][2],
    force_ptr[vert_id][0],force_ptr[vert_id][1],force_ptr[vert_id][2]);
    #endif

    vel_ptr[vert_id] += force_ptr[vert_id] / mass_ptr[vert_id] * static_cast<T>(dt);
    pos_ptr[vert_id] += vel_ptr[vert_id] * static_cast<T>(dt);

    #ifndef NDEBUG
    if(vert_id == 0)printf("after(%f,%f,%f) vel(%f,%f,%f)\n",pos_ptr[vert_id][0],pos_ptr[vert_id][1],pos_ptr[vert_id][2],
    vel_ptr[vert_id][0],vel_ptr[vert_id][1],vel_ptr[vert_id][2]);
    #endif
}

template<typename T>
void SymplecticEulerCuda<T>::advanceTime(float dt)
{
    SymplecticEulerCpu<T>::ptr_force_->setZeroGpu();
    Integrator<T>::internal_force_->internalForce(SymplecticEulerCpu<T>::ptr_pos_, SymplecticEulerCpu<T>::ptr_force_);
    Integrator<T>::velocity_damping_->addDampingForce(SymplecticEulerCpu<T>::ptr_vel_, SymplecticEulerCpu<T>::ptr_force_);
    Integrator<T>::gravity_->addGravityForce(SymplecticEulerCpu<T>::ptr_force_);
    Integrator<T>::fixed_freedom_->clearFixedFreedom(SymplecticEulerCpu<T>::ptr_force_);

    auto pos_ptr = SymplecticEulerCpu<T>::ptr_pos_->ptrGpu();
    auto vel_ptr = SymplecticEulerCpu<T>::ptr_vel_->ptrGpu();
    auto force_ptr = SymplecticEulerCpu<T>::ptr_force_->ptrGpu();
    auto mass_ptr = SymplecticEulerCpu<T>::ptr_mass_->ptrGpu();
    const int vert_num = SymplecticEulerCpu<T>::ptr_pos_->size();

    K_SymplecticEulerCuda_advanceTime<T> << <numBlocks(vert_num), BLOCK_SIZE >> > (pos_ptr, vel_ptr, force_ptr, mass_ptr, dt, vert_num);
    synchronCheck;
}

template class SymplecticEulerCuda<float>;
template class SymplecticEulerCuda<double>;

}